#include <cv.h>
#include <highgui.h>

#include "ros/ros.h"
#include <std_msgs/Int32.h>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "robo/CameraParams.h"
#include <linux/videodev2.h>
#include <libv4l2.h>
#include <string.h>
#include <signal.h>
#include "V4L2Camera.cc"

using namespace robo;
namespace enc = sensor_msgs::image_encodings;
const float max_speed = 0.03f;
int camera_interval = 1;
int camera_width 	= 640;
int camera_height	= 480;
bool sleep_stop 	= false;

bool cam0_on		= false;
bool cam1_on		= false;
V4L2Camera cam0;
V4L2Camera cam1;

bool run = true;
void sigtrap(int) {run = false;}


void change_camera_params(const CameraParams::ConstPtr &msg)
{
	sleep_stop = true;
	camera_interval = msg->interval;
	camera_width 	= msg->width;
	camera_height	= msg->height;
	printf("camera_interval: %i\n",camera_interval);
}



int main(int argc, char **argv)
{
	signal(SIGINT, sigtrap);
	
	printf("Starting CameraNode\n");
	ros::init(argc, argv, "CameraNode");
	ros::NodeHandle n;
	ros::Subscriber sub 	= n.subscribe("/camera_params"	, 100, change_camera_params);
	image_transport::ImageTransport it_(n);
	image_transport::Publisher img0_pub = it_.advertise("/camera0_img", 1);
	image_transport::Publisher img1_pub = it_.advertise("/camera1_img", 1);
	ros::Rate loop_rate(1000);
	
	cam0.connect("/dev/video0");

	
	
	if (cam0.init(V4L2Camera::SIZE_800_600, 1 )){
		cam0_on = true;
		printf("cam0 on\n");
	}else{printf("cam0 fail\n");}
	
	cam1.connect("/dev/video1");	
	if (cam1.init(V4L2Camera::SIZE_800_600, 1 )){
		cam1_on = true;
		printf("cam1 on\n");
	}else{printf("cam1 fail\n");}
	
	const bool debugg_send = false;
	if(debugg_send){
		if(cam0_on){cvNamedWindow("Cam0_send", CV_WINDOW_AUTOSIZE );}
		if(cam1_on){cvNamedWindow("Cam1_send", CV_WINDOW_AUTOSIZE );}
	}
	while(ros::ok() && run){
		
		
		IplImage * cam0_img;
		IplImage * cam1_img;
		
		if(cam0_on){
			if(debugg_send){printf("cam0.getIplImg()\n");}
			cam0_img = cam0.getIplImg();
			if(debugg_send){cvShowImage("Cam0_send", cam0_img);}
			
			cv::Mat mat(cam0_img);
			cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
			cv_ptr->image = mat;
			cv_ptr->encoding = enc::BGR8;
			img0_pub.publish(cv_ptr->toImageMsg());
		}
		if(cam1_on){
			if(debugg_send){printf("cam1.getIplImg()\n");}
			cam1_img = cam1.getIplImg();
			if(debugg_send){cvShowImage("Cam1_send", cam1_img);}
			
			cv::Mat mat(cam1_img);
			cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
			cv_ptr->image = mat;
			cv_ptr->encoding = enc::BGR8;
			img1_pub.publish(cv_ptr->toImageMsg());
		}
		if(debugg_send)	{if(cam0_on || cam1_on)	{cvWaitKey(30);}}
		if(cam0_on)		{cvReleaseImage( &cam0_img );}
		if(cam1_on)		{cvReleaseImage( &cam1_img );}
		
		ros::spinOnce();
		for(int i = 0; !sleep_stop && i < camera_interval;i++){loop_rate.sleep();}
		sleep_stop = false;
		ros::spinOnce();
	}
	
	if(cam0_on){
		printf("cam0 off\n");
		cam0.disconnect();
		cam0_on = false;
	}
	if(cam1_on){
		printf("cam1 off\n");
		cam1.disconnect();
		cam1_on = false;
	}
	return 0;
}
